//
// Created by 0 on 2023/1/2.
//

#include <math.h>
#include "driver_gimbal.h"
#include "task_communicate.h"
#include "bsp_can.h"
#include "driver_chassis.h"
#include "driver_communicate.h"

/* 裁判系统数据保护结构体 */
extern judgement_protection_struct judgement_protection;
extern ChassisSpeedMessegePort ChassisSpeed;

GimbalReceiveStruct GimbalReceive = {0};
extern int32_t LostCounterCountNumber[NUMBERS_OF_COUNT];

u8 CAN2SendGimbalMessegeBuffer1[8];
u8 CAN2SendGimbalMessegeBuffer2[8];
void SendtoGimbalMessage(void)
{

    //当裁判系统数据未丢失时发送裁判系统数据，当裁判系统丢失时发送保存数据
    if(judgement_protection.judgement_lost_flag == 0)
    {
        CAN2SendGimbalMessegeBuffer1[0] = ((int16_t)(judge_rece_mesg.robot_status_data.shooter_barrel_heat_limit)
                >> 8);//枪口热量上限
        CAN2SendGimbalMessegeBuffer1[1] = (
                (int16_t)(judge_rece_mesg.robot_status_data.shooter_barrel_heat_limit) & 0x00ff);
        CAN2SendGimbalMessegeBuffer1[2] = ((int16_t)(judge_rece_mesg.robot_status_data.shooter_barrel_cooling_value)
                >> 8);//枪口冷却值
        CAN2SendGimbalMessegeBuffer1[3] = ((int16_t)(judge_rece_mesg.robot_status_data.shooter_barrel_cooling_value) &
                                           0x00ff);
        //4,5为射速上限
        CAN2SendGimbalMessegeBuffer1[4] = ((int16_t)(30) >> 8);
        CAN2SendGimbalMessegeBuffer1[5] = ((int16_t)(30) & 0x00ff);
        CAN2SendGimbalMessegeBuffer1[6] = (int8_t)(judge_rece_mesg.robot_status_data.robot_id);//机器人id
    }
    else
    {
        CAN2SendGimbalMessegeBuffer1[0] = ((int16_t)(judgement_protection.shooter_barrel_heat_limit)
                >> 8);//枪口热量上限
        CAN2SendGimbalMessegeBuffer1[1] = (
                (int16_t)(judgement_protection.shooter_barrel_heat_limit) & 0x00ff);
        CAN2SendGimbalMessegeBuffer1[2] = ((int16_t)(judgement_protection.shooter_barrel_cooling_value)
                >> 8);//枪口冷却值
        CAN2SendGimbalMessegeBuffer1[3] = ((int16_t)(judgement_protection.shooter_barrel_cooling_value) &
                                           0x00ff);
        CAN2SendGimbalMessegeBuffer1[4] = ((int16_t)(30) >> 8);//枪口射速上限
        CAN2SendGimbalMessegeBuffer1[5] = ((int16_t)(30) & 0x00ff);
        CAN2SendGimbalMessegeBuffer1[6] = (int8_t)(judgement_protection.robot_id);//机器人id
    }

    if(LostCounterCountNumber[JUDGEMENT_LOST_COUNT]>(JUDGEMENT_LOST_TOLERANCE_MS/LOST_COUNTER_TIME_MS))
        CAN2SendGimbalMessegeBuffer1[7] = (CAN2SendGimbalMessegeBuffer1[7] & (~(1 << 0)));
    else
        CAN2SendGimbalMessegeBuffer1[7] = (CAN2SendGimbalMessegeBuffer1[7] | (1 << 0));
    if(judge_rece_mesg.robot_status_data.mains_power_shooter_output)
        CAN2SendGimbalMessegeBuffer1[7] = (CAN2SendGimbalMessegeBuffer1[7] | (1 << 1));
    else
        CAN2SendGimbalMessegeBuffer1[7] = (CAN2SendGimbalMessegeBuffer1[7] & (~(1 << 1)));

    CAN2SendGimbalMessegeBuffer2[0] =
            ((int16_t)(judge_rece_mesg.power_heat_data.shooter_17mm_1_barrel_heat)) >> 8;//枪口热量1
    CAN2SendGimbalMessegeBuffer2[1] =
            ((int16_t)(judge_rece_mesg.power_heat_data.shooter_17mm_1_barrel_heat)) & 0x00ff;
    CAN2SendGimbalMessegeBuffer2[4] =
            ((int16_t)(judge_rece_mesg.power_heat_data.shooter_17mm_2_barrel_heat)) >> 8;//枪口热量2
    CAN2SendGimbalMessegeBuffer2[5] =
            ((int16_t)(judge_rece_mesg.power_heat_data.shooter_17mm_2_barrel_heat)) & 0x00ff;

    CAN2SendGimbalMessegeBuffer2[2] = ((int16_t)(judge_rece_mesg.shoot_data.bullet_speed * 100)) >> 8;//子弹速度
    CAN2SendGimbalMessegeBuffer2[3] = ((int16_t)(judge_rece_mesg.shoot_data.bullet_speed * 100)) & 0x00ff;
    CAN2SendGimbalMessegeBuffer2[6] = judge_rece_mesg.robot_status_data.mains_power_shooter_output;
    CAN2SendGimbalMessegeBuffer2[7] = ((int8_t)(ChassisSpeed.Spin * 100 + 100)) & 0x00ff;//小陀螺速度

    CAN2_Send_Msg(CAN2SendGimbalMessegeBuffer1, 8, 0x221);
    CAN2_Send_Msg(CAN2SendGimbalMessegeBuffer2,8,0x222);
}


